#pragma once

#include <iostream>
#include <queue>
#include <vector>
#include <algorithm>
#include <cmath>
#include "AgvCar.hpp"
#include "Task.hpp"
#include"pathplanner.hpp"

const int defaultSize = 20;

class Scheduler
{
    const int defaultnum = 2;

private:
    // 两个队列，用于存放待调度的任务；一个为正常队列，一个为优先级高的队列
    std::queue<Task> NormalTask_;
    // std::queue<int> PriorityTask_;
    // 管理小车类
    std::vector<Agv> agvs_;
    std::vector<std::vector<int>> grid;

public:
    void AddTask(const Task &task)
    {
        NormalTask_.push(task);
    }

    Scheduler(std::vector<std::vector<int>> &gd)
    {
        // 创建小车
        for (int i = 0; i < defaultnum; ++i)
        {
            agvs_.push_back(Agv(Point(5, 5)));
            std::cout << "agv:" << i << std::endl;
        }
        grid = gd;
        // grid[3][3] = 1;
    }

    int CalMinDistance(Point start, Point end)
    {
        return abs(start.x - end.x) + abs(start.y - end.y);
    }

    std::vector<Point> pathPlanning(Point start, Point end)
    {
        // 使用A*算法进行路径规划，考虑障碍物和其他AGV的位置
        std::vector<Point> path;
        int dx = end.x - start.x;
        int dy = end.y - start.y;
        //算出最大需要多少步
        int steps = std::max(abs(dx), abs(dy));
        for (int i = 0; i < steps; ++i)
        {
            int x = start.x + dx * i / steps;
            int y = start.y + dy * i / steps;
            if (x < 0 || x >= defaultSize || y < 0 || y >= defaultSize || grid[x][y] == 1)
            {
                // 越界或遇到障碍物，无法直线通过，重新规划路径
                path.clear();
                break;
            }
            path.push_back(Point(x, y));
        }
        return path;
    }

    void Start()
    {
        // 循环判断任务队列中是否有任务，有任务分配给距离最近的可用小车
        while (!NormalTask_.empty())
        {
            // 分配小车,遍历所有可用小车，计算所有的距离，找出距离最小
            Task ExecuateTask = NormalTask_.front();
            NormalTask_.pop();

            Point start(ExecuateTask.StationPos_);
            Point end(ExecuateTask.EndPos_);
            int index = 0;
            int MinDistance = 0x3f3f3f3f;
            for (int i = 0; i < agvs_.size(); ++i)
            {
                // 计算最近距离，找出下标
                int distance = CalMinDistance(start, end);
                if (distance < MinDistance)
                {
                    MinDistance = distance;
                    index = i;
                }
            }
            // 找到需要调度的小车
            // 进行调度
            Agv targetAgv = agvs_[index];
            PathPlanner planner(grid,start,end);
            std::vector<Point> path = planner.findPath();
            if(!path.empty())
            {
                std::cout<<"找到路径"<<std::endl;
                for(int i = 0;i<path.size();++i)
                {
                    std::cout << "(" << path[i].x << "," << path[i].y << ") ";
                }
                std::cout<<std::endl;


                targetAgv.setPath(path);
                while(targetAgv.GetStatus())
                {
                    targetAgv.moveTo();
                }

            }
            else
            {
                std::cout<<"找不到可用路径"<<std::endl;
            }
            // std::vector<Point> path = pathPlanning(targetAgv.getPosition(), ExecuateTask.StationPos_);

            // targetAgv.setPath(path);
            // for (const Point &p : path)
            // {
            //     targetAgv.moveTo();
            // }
            // std::cout << "**************************" << std::endl;
            // path = pathPlanning(ExecuateTask.StationPos_, ExecuateTask.EndPos_);
            // targetAgv.setPath(path);
            // for (const Point &p : path)
            // {
            //     targetAgv.moveTo();
            // }
        }
    }
};